#include <Wire.h>
#include <ADXL345.h>
#include <L3G4200D.h>
#include <EasyTransfer.h>
#include <digitalWriteFast.h>
#include "BalancingRobot.h"

ADXL345 acc;
L3G4200D gyro;

void setup() 
{
  /* Setup encoders */
  pinMode(leftEncoder1,  INPUT);
  pinMode(leftEncoder2,  INPUT);
  pinMode(rightEncoder1, INPUT);
  pinMode(rightEncoder2, INPUT); 

  attachInterrupt(0, leftEncoder,  RISING); // pin 2
  attachInterrupt(1, rightEncoder, RISING); // pin 3

  InicializaConfPinos();

  Serial.begin(115200);

  delay(100);

  Serial1.begin(115200);  

  delay(100);

  gyro.begin();

  delay(100);

  acc.begin();

  delay(100);

  Receptor.begin(details(rxdata), &Serial1);

  delay(200);

  rxdata.Sequencial = -1;

  /* Setup pin for buzzer to beep when finished calibrating */
  //pinMode(buzzer,OUTPUT);  

  /* Setup IMU Inputs */
  //analogReference(EXTERNAL); // Set voltage reference to 3.3V by connecting AREF to 3.3V

  delay(200);

  //fwhile (! VerificaControleLigado()); // Halt

  /* Calibrate the gyro and accelerometer relative to ground */
  calibrateSensors();

  stopAndReset();

  CalibraControle();

//  for (int i = 1; i <= 5; i++)
//  {
//    digitalWriteFast(PinoLed, HIGH);
//    delay(200);
//    digitalWriteFast(PinoLed, LOW);
//    delay(200);
//  }    

  /* Setup timing */
  loopStartTime = micros();
  timer = loopStartTime;
}

void loop() 
{
  /* Calculate pitch */ 
  accYangle = getAccY();
  gyroYrate = getGyroYrate();

  // See my guide for more info about calculation the angles and the Kalman filter: http://arduino.cc/forum/index.php/topic,58048.0.htm
  pitch = kalman(accYangle, gyroYrate, (double)(micros() - timer)); // calculate the angle using a Kalman filter

  Serial.print("  accYangle: ");
  Serial.print(accYangle);
  
  Serial.print("  gyroYrate: ");
  Serial.print(gyroYrate);

  Serial.print("  pitch: ");
  Serial.println(pitch);

  timer = micros();  

  /* Drive motors */
  // If the robot is laying down, it has to be put in a vertical position before it starts balancing
  // If it's already balancing it has to be ±45 degrees before it stops trying to balance
  if ((layingDown && (pitch < 170 || pitch > 190)) || (!layingDown && (pitch < 135 || pitch > 225))) 
  {
    layingDown = true; // The robot is in a unsolvable position, so turn off both motors and wait until it's vertical again
    stopAndReset();
  } 
  else 
  {
    layingDown = false; // It's no longer laying down
    PID(targetAngle, targetOffset, turningOffset);  
  }

  /* Update wheel velocity every 100ms */
  loopCounter++;

  if (loopCounter == 10) 
  {
    loopCounter = 0; // Reset loop counter
    wheelPosition = readLeftEncoder() + readRightEncoder();

    wheelVelocity = wheelPosition - lastWheelPosition;

    lastWheelPosition = wheelPosition;

    if (abs(wheelVelocity) <= 20 && !stopped)  // Set new targetPosition if breaking
    {
      targetPosition = wheelPosition;
      stopped = true;
    }
  }

  /* Read the PS3 Controller */
  receivePS3();

  /* Use a time fixed loop */
  lastLoopUsefulTime = micros() - loopStartTime;

  if (lastLoopUsefulTime < STD_LOOP_TIME)
    delayMicroseconds(STD_LOOP_TIME - lastLoopUsefulTime);

  loopStartTime = micros();
}

void PID(double restAngle, double offset, double turning) 
{
  /* Steer robot */
  if (steerForward) 
    {
      offset += (double)wheelVelocity / velocityScaleMove; // Scale down offset at high speed and scale up when reversing
      restAngle -= offset;
    } 
  else if (steerBackward) 
    {
      offset -= (double)wheelVelocity / velocityScaleMove; // Scale down offset at high speed and scale up when reversing
      restAngle += offset;
    }
  /* Break */
  else if (steerStop) 
    {
      long positionError = wheelPosition - targetPosition;

      if (abs(positionError) > zoneA) // Inside zone A
        restAngle -= (double)positionError / positionScaleA;

      else if (abs(positionError) > zoneB) // Inside zone B
        restAngle -= (double)positionError / positionScaleB;

      else // Inside zone C
        restAngle -= (double)positionError / positionScaleC; 

      restAngle -= (double)wheelVelocity / velocityScaleStop;

      if (restAngle < 160) // Limit rest Angle
        restAngle = 160;

      else if (restAngle > 200)
        restAngle = 200;
  }

  /* Update PID values */
  double error = (restAngle - pitch);
  double pTerm = Kp * error;
  iTerm += Ki * error;
  double dTerm = Kd * (error - lastError);
  lastError = error;

  double PIDValue = pTerm + iTerm + dTerm;

  /* Steer robot sideways */
  double PIDLeft;
  double PIDRight;

  if (steerLeft) 
  {
    turning -= abs((double)wheelVelocity / velocityScaleTurning); // Scale down at high speed

    if (turning < 0)
      turning = 0;

    PIDLeft  = PIDValue - turning;
    PIDRight = PIDValue + turning;
  }
  else if (steerRight) 
  {
    turning -= abs((double)wheelVelocity / velocityScaleTurning); // Scale down at high speed

    if(turning < 0)
      turning = 0;

    PIDLeft  = PIDValue + turning;
    PIDRight = PIDValue - turning;
  }
  else 
  {
    PIDLeft  = PIDValue;
    PIDRight = PIDValue;
  }

  PIDLeft *= 0.95; // compensate for difference in the motors

  //Serial.print("PIDValue: ");
  //Serial.println(PIDValue);

//  Serial.print("    turning: ");
//  Serial.print(turning);
//
//  Serial.print("    PIDLeft: ");
//  Serial.print(PIDLeft);
//
//  Serial.print("    PIDRight: ");
//  Serial.println(PIDRight);

  /* Set PWM Values */
  if (PIDLeft >= 0)
    moveMotor(left, forward, PIDLeft);
  else
    moveMotor(left, backward, PIDLeft);

  if (PIDRight >= 0)
    moveMotor(right, forward, PIDRight);
  else
    moveMotor(right, backward, PIDRight);
}

void receivePS3() 
{
  // Set all false
  steerForward  = false;
  steerBackward = false;
  steerStop     = false;
  steerLeft     = false;
  steerRight    = false;

  Receptor.receiveData();

  ReadThrotle();

  ReadAileron();

  if ((adjvalorAileron > 200) || (adjvalorAileron < 55) || (adjvalorThrotle > 137) || (adjvalorThrotle < 117)) 
    {
      steer(update);
    } 
  else 
    steer(stop);      

  //Serial.print("adjvalorThrotle: ");
  //Serial.print(adjvalorThrotle);

  //Serial.print("    adjvalorAileron: ");
  //Serial.println(adjvalorAileron);      

}

void steer(Command command) 
{
  targetOffset  = 0;
  turningOffset = 0;

  if (adjvalorThrotle < 117) 
    {
      targetOffset = scale(adjvalorThrotle,116,0,255); // Scale from 116-0 to 0-7
      steerForward = true;
    } 
  else if (adjvalorThrotle > 137) 
    {
      targetOffset = scale(adjvalorThrotle,138,0,255); // Scale from 138-255 to 0-7
      steerBackward = true;
    }

  if (adjvalorAileron < 55) 
    {
    turningOffset = scale(adjvalorAileron,54,0,20); // Scale from 54-0 to 0-20
    steerLeft = true;     
    } 
  else if (adjvalorAileron > 200) 
    {
      turningOffset = scale(adjvalorAileron,201,255,20); // Scale from 201-255 to 0-20
      steerRight = true;
    }

//  Serial.print("targetOffset: ");
//  Serial.print(targetOffset);
//
//  Serial.print("    turningOffset: ");
//  Serial.println(turningOffset);   

  if (command == stop) 
  {
    steerStop = true;  

    if (lastCommand != stop) 
    { // Set new stop position
      targetPosition = wheelPosition;
      stopped = false;
    }
  }

  lastCommand = command;
}

double scale(double input, double inputMin, double inputMax, double outputMax)  // Like map() just returns a double
{
  if (inputMin < inputMax)
    return (input-inputMin) / ((inputMax-inputMin)/outputMax);              
  else
    return (inputMin-input) / ((inputMin-inputMax)/outputMax);    
}

void stopAndReset() 
{
  stopMotor(left);
  stopMotor(right);  
  lastError = 0;
  iTerm = 0;
  targetPosition = wheelPosition;
}

double kalman(double newAngle, double newRate, double dtime) 
{
  // KasBot V2  -  Kalman filter module - http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1284738418
  // See also http://www.x-firm.com/?page_id=145
  // with slightly modifications by Kristian Lauszus
  // See http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf and 
  // http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf for more information
  dt = dtime / 1000000; // Convert from microseconds to seconds

  // Discrete Kalman filter time update equations - Time Update ("Predict")
  // Update xhat - Project the state ahead
  angle += dt * (newRate - bias);

  // Update estimation error covariance - Project the error covariance ahead
  P_00 += -dt * (P_10 + P_01) + Q_angle * dt;
  P_01 += -dt * P_11;
  P_10 += -dt * P_11;
  P_11 += +Q_gyro * dt;

  // Discrete Kalman filter measurement update equations - Measurement Update ("Correct")
  // Calculate Kalman gain - Compute the Kalman gain
  S = P_00 + R_angle;
  K_0 = P_00 / S;
  K_1 = P_10 / S;

  // Calculate angle and resting rate - Update estimate with measurement zk
  y = newAngle - angle;
  angle += K_0 * y;
  bias += K_1 * y;

  // Calculate estimation error covariance - Update the error covariance
  P_00 -= K_0 * P_00;
  P_01 -= K_0 * P_01;
  P_10 -= K_1 * P_00;
  P_11 -= K_1 * P_01;

  return angle;
}

double getGyroYrate() 
{
  // (gyroAdc-gyroZero)/Sensitivity (In quids) - Sensitivity = 0.00333/3.3*1023=1.0323  

  //double gyroRate = -((double)((double)analogRead(gyroY) - zeroValues[0]) / 1.0323);

  double gyroRate = -((double)((double)gyro.getY() - zeroValues[0]) / 1.0323);

  return gyroRate;
}

double getAccY() 
{
  //double accYval = ((double)analogRead(accY) - zeroValues[1]);  
  //double accZval = ((double)analogRead(accZ) - zeroValues[2]);

  acc.read();

  double accYval = ((double)acc.getRawY() - zeroValues[1]);  
  double accZval = ((double)acc.getRawZ() - zeroValues[2]);

//  Serial.print("  accYval: ");
//  Serial.print(accYval);
//  
//  Serial.print("  accZval: ");
//  Serial.printl(accZval);  
//  
  // Convert to 360 degrees resolution
  // atan2 outputs the value of -π to π (radians) - see http://en.wikipedia.org/wiki/Atan2
  // We are then convert it to 0 to 2π and then from radians to degrees
  return (atan2(-accYval, -accZval) + PI) * RAD_TO_DEG;
}

void calibrateSensors() 
{
  zeroValues[0] = 0;
  zeroValues[1] = 0;
  zeroValues[2] = 0;

  for (uint8_t i = 0; i < 100; i++) 
  { // Take the average of 100 readings

    //zeroValues[0] += analogRead(gyroY);
    //zeroValues[1] += analogRead(accY);
    //zeroValues[2] += analogRead(accZ);

    acc.read();

    zeroValues[0] += gyro.getY();
    zeroValues[1] += acc.getRawY();
    zeroValues[2] += acc.getRawZ();

    delay(10);
  }

  zeroValues[0] /= 100; // Gyro X-axis
  zeroValues[1] /= 100; // Accelerometer Y-axis
  zeroValues[2] /= 100; // Accelerometer Z-axis

  if (zeroValues[1] > 500)  // Check which side is lying down - 1g is equal to 0.33V or 102.3 quids (0.33/3.3*1023=102.3)
  {  
    zeroValues[1] -= 102.3; // -1g when lying at one of the sides
    angle = 90; // It starts at 90 degress and 270 when facing the other way
  } 
  else 
  {
    zeroValues[1] += 102.3; // +1g when lying at the other side
    angle = 270;
  }

  //digitalWriteFast(buzzer,HIGH);
  //delay(100);  
  //digitalWriteFast(buzzer,LOW);
}

void moveMotor(Command motor, Command direction, double speedRaw)  // speed is a value in percentage 0-100%
{
  if (speedRaw > 100)
    speedRaw = 100;

  if (direction == forward) 
    speedRaw = 100 - speedRaw;
  
  int speed = speedRaw * ((double)PWMVALUE) / 100; // Scale from 100 to PWMVALUE

  if (motor == left) 
  {
    if (direction == forward) 
    {
      digitalWrite(PoM1, HIGH);
      analogWrite(PWM1, speed);
    } 
    else if (direction == backward) 
    {
      digitalWrite(PoM1, LOW);
      analogWrite(PWM1, speed);

    }
  } 
  else if (motor == right) 
    {
    if (direction == forward) 
    {
      digitalWrite(PoM2, HIGH);
      analogWrite(PWM2, speed);
    } 
    else if (direction == backward) 
    {
      digitalWrite(PoM2, LOW);
      analogWrite(PWM2, speed);
    }
  }
}

void stopMotor(Command motor) 
{  
  if (motor == left) 
  {
    //setPWM(leftPWM,PWMVALUE); // Set high
    //sbi(leftPort,leftA);
    //sbi(leftPort,leftB);

    digitalWrite(PWM1, 0);
    digitalWrite(PoM1, LOW);
  } 
  else if (motor == right) 
  {
    //setPWM(rightPWM, PWMVALUE); // Set high
    //sbi(rightPort, rightA);
    //sbi(rightPort, rightB);

    digitalWrite(PWM2, 0);
    digitalWrite(PoM2, LOW);
  }
}

void setPWM(uint8_t pin, int dutyCycle)  // dutyCycle is a value between 0-ICR
{
  if (pin == leftPWM) 
  {
    OCR1AH = (dutyCycle >> 8); 
    OCR1AL = (dutyCycle & 0xFF);
  } 
  else if (pin == rightPWM) 
  {
    OCR1BH = (dutyCycle >> 8);
    OCR1BL = (dutyCycle & 0xFF);    
  }
}

/* Interrupt routine and encoder read functions - I read using the port registers for faster processing */
void leftEncoder() 
{ 
  //if (PIND & _BV(PIND4)) // read pin 4

  if (digitalReadFast(leftEncoder1) != digitalReadFast(leftEncoder2)) // read pin 4
    leftCounter++;
  else
    leftCounter--;

  //Serial.print("  leftCounter: ");
  //Serial.println(leftCounter);

}

void rightEncoder() 
{
 // if(PIND & _BV(PIND5)) // read pin 5
  if (digitalReadFast(rightEncoder1) != digitalReadFast(rightEncoder2)) // read pin 4
    rightCounter++;
  else
    rightCounter--;

  //Serial.print("rightCounter: ");
  //Serial.print(rightCounter);

  //Serial.print("  leftCounter: ");
  //Serial.println(leftCounter);

}


long readLeftEncoder()  // The encoders decrease when motors are traveling forward and increase when traveling backward
{
  return leftCounter;
}

long readRightEncoder() 
{
  return rightCounter;
}

int VerificaControleLigado() 
{
  Receptor.receiveData();
  return (rxdata.Sequencial != -1);

  delay(10);
}

void CalibraControle()
{
  for (int i = 0; i < 50; i++)
  {
    Receptor.receiveData();

    ReadThrotle();

    if (adjvalorThrotle < vlMinDeadZoneThrotle)
      vlMinDeadZoneThrotle = adjvalorThrotle - 1;

    if (adjvalorThrotle > vlMaxDeadZoneThrotle)
      vlMaxDeadZoneThrotle = adjvalorThrotle + 1;

    ReadAileron();

    if (adjvalorAileron < vlMinDeadZoneAileron)
      vlMinDeadZoneAileron = adjvalorAileron - 1;

    if (adjvalorAileron > vlMaxDeadZoneAileron)
      vlMaxDeadZoneAileron = adjvalorAileron + 1;

    delay(10);
  }

  vlMidDeadZoneThrotle =  vlMinDeadZoneThrotle + ((vlMaxDeadZoneThrotle - vlMinDeadZoneThrotle) / 2);  
  vlMidDeadZoneAileron =  vlMinDeadZoneAileron + ((vlMaxDeadZoneAileron - vlMinDeadZoneAileron) / 2); 

//  Serial.print("vlMidDeadZoneThrotle: ");
//  Serial.print(vlMidDeadZoneThrotle);
//
//  Serial.print("    vlMidDeadZoneAileron: ");
//  Serial.println(vlMidDeadZoneAileron);  

}

void ReadThrotle()
{
  adjvalorThrotle = map(rxdata.Throtle, 0, 1023, 0, 254); 

  //adjvalorThrotle = 127;

  constrain (adjvalorThrotle, 0, 254);
}  

void ReadAileron()
{
  adjvalorAileron = map(rxdata.Ailevon, 0, 1023, 0, 254);

  //adjvalorAileron = 127;

  constrain(adjvalorAileron, 0, 254);
}

void InicializaConfPinos()
{
  pinMode(ENA, OUTPUT); 
  pinMode(ENB, OUTPUT); 

  pinMode(PWM1, OUTPUT); 
  pinMode(PoM1, OUTPUT); 

  pinMode(PWM2, OUTPUT);   
  pinMode(PoM2, OUTPUT);   

  digitalWrite(PoM1, LOW) ;   // Both motor with same polarity
  digitalWrite(PoM2, LOW) ;

  analogWrite(PWM1, 0);   // Stop both motors => ValMx = 0
  analogWrite(PWM2, 0);    

  digitalWrite(ENA, HIGH);
  digitalWrite(ENB, HIGH);    
}
